#include <iostream>
#include <vector>

#include "CGL/vector2D.h"

#include "mass.h"
#include "rope.h"
#include "spring.h"

namespace CGL {

    Rope::Rope(Vector2D start, Vector2D end, int num_nodes, float node_mass, float k, vector<int> pinned_nodes)
    {
        // TODO (Part 1): Create a rope starting at `start`, ending at `end`, and containing `num_nodes` nodes.

//        Comment-in this part when you implement the constructor
//        for (auto &i : pinned_nodes) {
//            masses[i]->pinned = true;
//        }

        auto step = (end - start) / (num_nodes-1);
        for (int i = 0; i < num_nodes; ++i) {
            Mass* tempMass = new Mass(start + step * i, node_mass, false);
            tempMass->velocity = { 0,0 };
            masses.push_back(tempMass);
            if (i > 0) {
                Spring* tempSpring = new Spring(masses[i - 1], masses[i], k);
                springs.push_back(tempSpring);
            }
        }
        for (auto node : pinned_nodes) {
            masses[node]->pinned = true;
        }
    }

    void Rope::simulateEuler(float delta_t, Vector2D gravity)
    {
        for (auto &s : springs)
        {
            // TODO (Part 2): Use Hooke's law to calculate the force on a node
            auto dir = s->m2->position - s->m1->position;
            s->m1->forces += s->k *dir.unit()*(dir.norm() - s->rest_length);
            s->m2->forces += s->k * dir.unit() * (dir.norm() - s->rest_length) * -1.f;
        }

        for (auto &m : masses)
        {
            if (!m->pinned)
            {
                // TODO (Part 2): Add the force due to gravity, then compute the new velocity and position
                m->forces += m->mass * gravity;
                float k_d = 0.01f;
                Vector2D f_d = -k_d * m->velocity;
                m->forces += f_d;
                auto a = m->forces / m->mass;
                m->velocity += delta_t * a;
                m->position += m->velocity * delta_t;
                // TODO (Part 2): Add global damping
            }

            // Reset all forces on each mass
            m->forces = Vector2D(0, 0);
        }
    }

    void Rope::simulateVerlet(float delta_t, Vector2D gravity)
    {
        for (auto &s : springs)
        {
            // TODO (Part 3): Simulate one timestep of the rope using explicit Verlet （solving constraints)
            auto dir = s->m2->position - s->m1->position;
            s->m1->forces += s->k * dir.unit() * (dir.norm() - s->rest_length);
            s->m2->forces += s->k * dir.unit() * (dir.norm() - s->rest_length) * -1.f;
        }

        for (auto &m : masses)
        {
            if (!m->pinned)
            {
                Vector2D temp_position = m->position;
                // TODO (Part 3.1): Set the new position of the rope mass
                m->forces += m->mass * gravity;
                auto a = m->forces / m->mass;
                auto last_position = m->position;
                float damping = 0.000005f;
                m->position += (1 - damping)* (m->position - m->last_position) + a * delta_t * delta_t;
                m->last_position = last_position;
                // TODO (Part 4): Add global Verlet damping
            }
            m->forces = Vector2D(0, 0);
        }
    }
}
